#ifndef CURVEFITTING_HPP
#define CURVEFITTING_HPP

#include <stdexcept>
#include <cmath>
#include <vector>
#include <iostream>
#include "Spline.hpp"

// 计算两点之间的距离
double distance(double x1, double y1, double x2, double y2) {
    return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

double distance3(double x1, double y1, double z1, double x2, double y2,double z2) {
    return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)+(z1-z2)*(z1-z2));
}

std::vector<std::vector<double>> Curve_Fitting_cumulativechordal_length(
    const std::vector<double>& x_vals, 
    const std::vector<double>& y_vals,
    const std::vector<double>& t_vals
) {
    if (x_vals.size() != y_vals.size() || x_vals.size() != t_vals.size()) {
        throw std::invalid_argument("Input vectors must have the same size.");
    }

    size_t n = x_vals.size();
    double length = 0.0;

    // 计算总弦长
    for (size_t i = 1; i < n; ++i) {
        length += distance(x_vals[i - 1], y_vals[i - 1], x_vals[i], y_vals[i]);
    }

    // 参数化变量 t
    std::vector<double> t(n, 0.0);
    t[0] = 0.0;
    for (size_t i = 1; i < n; ++i) {
        t[i] = t[i - 1] + distance(x_vals[i - 1], y_vals[i - 1], x_vals[i], y_vals[i]) / length;
    }

    // 使用 B 样条拟合 x 和 y
    BSpline spline_x(t, x_vals, 3, "periodic");
    BSpline spline_y(t, y_vals, 3, "periodic");

    // 结果存储
    std::vector<std::vector<double>> result(2);
    for (double s = 0.0; s <= 1.0; s += 0.01) {
        result[0].push_back(spline_x.getSplineValue(s));
        result[1].push_back(spline_y.getSplineValue(s));
    }

    return result;
}

std::vector<std::vector<double>> Curve_Fitting_Equidistantnode(
    const std::vector<double>& x_vals, 
    const std::vector<double>& y_vals,
    const std::vector<double>& t_vals
) {
    if (x_vals.size() != y_vals.size() || x_vals.size() != t_vals.size()) {
        throw std::invalid_argument("Input vectors must have the same size.");
    }

    // 使用 B 样条拟合 x 和 y
    BSpline spline_x(t_vals, x_vals, 3, "periodic");
    BSpline spline_y(t_vals, y_vals, 3, "periodic");

    // 结果存储
    std::vector<std::vector<double>> result(2);
    for (double s = t_vals[0]; s <= t_vals[t_vals.size()-1]; s += 0.01) {
        result[0].push_back(spline_x.getSplineValue(s));
        result[1].push_back(spline_y.getSplineValue(s));
    }

    return result;
}


std::vector<std::vector<double>> Curve_Fitting_cumulativechordal_length_r2(
    const std::vector<double>& x_vals, 
    const std::vector<double>& y_vals,
    const std::vector<double>& t_vals
) {
    if (x_vals.size() != y_vals.size() || x_vals.size() != t_vals.size()) {
        throw std::invalid_argument("Input vectors must have the same size.");
    }

    size_t n = x_vals.size();
    double length = 0.0;

    // 计算总弦长
    for (size_t i = 1; i < n; ++i) {
        length += distance(x_vals[i - 1], y_vals[i - 1], x_vals[i], y_vals[i]);
    }

    // 参数化变量 t
    std::vector<double> t(n, 0.0);
    t[0] = 0.0;
    for (size_t i = 1; i < n; ++i) {
        t[i] = t[i - 1] + distance(x_vals[i - 1], y_vals[i - 1], x_vals[i], y_vals[i]) / length;
    }

    // 使用 B 样条拟合 x 和 y
    BSpline spline_x(t, x_vals, 3, "natural");
    BSpline spline_y(t, y_vals, 3, "natural");

    // 结果存储
    std::vector<std::vector<double>> result(2);
    for (double s = 0.0; s <= 1.0; s += 0.01) {
        result[0].push_back(spline_x.getSplineValue(s));
        result[1].push_back(spline_y.getSplineValue(s));
    }

    return result;
}

std::vector<std::vector<double>> Curve_Fitting_Equidistantnode_r2(
    const std::vector<double>& x_vals, 
    const std::vector<double>& y_vals,
    const std::vector<double>& t_vals
) {
    if (x_vals.size() != y_vals.size() || x_vals.size() != t_vals.size()) {
        throw std::invalid_argument("Input vectors must have the same size.");
    }

    // 使用 B 样条拟合 x 和 y
    BSpline spline_x(t_vals, x_vals, 3, "natural");
    BSpline spline_y(t_vals, y_vals, 3, "natural");

    // 结果存储
    std::vector<std::vector<double>> result(2);
    for (double s = t_vals[0]; s <= t_vals[t_vals.size()-1]; s += 0.01) {
        result[0].push_back(spline_x.getSplineValue(s));
        result[1].push_back(spline_y.getSplineValue(s));
    }

    return result;
}

std::vector<std::vector<double>> Curve_Fitting_r3_cumulativechordal_length(
    const std::vector<double>& x_vals, 
    const std::vector<double>& y_vals,
    const std::vector<double>& z_vals,
    const std::vector<double>& t_vals
) {
    if (x_vals.size() != y_vals.size() || x_vals.size() != t_vals.size()|| x_vals.size() != z_vals.size()) {
        throw std::invalid_argument("Input vectors must have the same size.");
    }

    size_t n = x_vals.size();
    double length = 0.0;

    // 计算总弦长
    for (size_t i = 1; i < n; ++i) {
        length += distance3(x_vals[i - 1], y_vals[i - 1], z_vals[i - 1],x_vals[i], y_vals[i],z_vals[i]);
    }

    // 参数化变量 t
    std::vector<double> t(n, 0.0);
    t[0] = 0.0;
    for (size_t i = 1; i < n; ++i) {
        t[i] = t[i - 1] + distance3(x_vals[i - 1], y_vals[i - 1], z_vals[i - 1],x_vals[i], y_vals[i],z_vals[i]) / length;
    }

    // 使用 B 样条拟合 x 和 y
    BSpline spline_x(t, x_vals, 3, "periodic");
    BSpline spline_y(t, y_vals, 3, "periodic");
    BSpline spline_z(t, z_vals, 3, "periodic");

    // 结果存储
    std::vector<std::vector<double>> result(3);
    for (double s = 0.0; s <= 1.0; s += 0.01) {
        result[0].push_back(spline_x.getSplineValue(s));
        result[1].push_back(spline_y.getSplineValue(s));
        result[2].push_back(spline_z.getSplineValue(s));
    }

    return result;
}

std::vector<std::vector<double>> Curve_Fitting_r3_Equidistantnode(
    const std::vector<double>& x_vals, 
    const std::vector<double>& y_vals,
    const std::vector<double>& z_vals,
    const std::vector<double>& t_vals
) {
    if (x_vals.size() != y_vals.size() || x_vals.size() != t_vals.size()|| x_vals.size() != z_vals.size()) {
        throw std::invalid_argument("Input vectors must have the same size.");
    }

    // 使用 B 样条拟合 x 和 y
    BSpline spline_x(t_vals, x_vals, 3, "periodic");
    BSpline spline_y(t_vals, y_vals, 3, "periodic");
    BSpline spline_z(t_vals, z_vals, 3, "periodic");

    // 结果存储
    std::vector<std::vector<double>> result(3);
    for (double s = t_vals[0]; s <= t_vals[t_vals.size()-1]; s += 0.01) {
        result[0].push_back(spline_x.getSplineValue(s));
        result[1].push_back(spline_y.getSplineValue(s));
        result[2].push_back(spline_z.getSplineValue(s));
    }

    return result;
}

#endif 
